#include "eepromManager.h"
#include <Arduino.h>
#include <avr/eeprom.h>
#include <string.h>
#include <stdint.h>

#define EEPROM_OFFSET_PID 0
#define EEPROM_OFFSET_SVO (EEPROM_OFFSET_PID + sizeof(SpidConstants))

void EEPROMmanager::writePIDconstants(SpidConstants input)
{writeTemplate<SpidConstants>(input,EEPROM_OFFSET_PID);}
SpidConstants EEPROMmanager::readPIDconstants()
{return readTemplate<SpidConstants>(EEPROM_OFFSET_PID);}

void EEPROMmanager::writeServoCalibrations(const SservoAllCalib &calib)
{writeTemplate<SservoAllCalib>(calib,EEPROM_OFFSET_SVO);}
SservoAllCalib EEPROMmanager::readServoCalibrations()
{return readTemplate<SservoAllCalib>(EEPROM_OFFSET_SVO);}

template <class T>
void EEPROMmanager::writeTemplate(const T &input, unsigned int pos)
{
	uint8_t data[sizeof(T)];
	memcpy(data, &input, sizeof(T));
	for (unsigned int i=0;i<sizeof(T);i++)
	{eeprom_write_byte((uint8_t *) (pos+i), data[i]);}
}

template <class T>
T EEPROMmanager::readTemplate(unsigned int pos)
{
	T bin;
	uint8_t data[sizeof(T)];
	for (unsigned int i=0;i<sizeof(T);i++)
	{data[i] = eeprom_read_byte((uint8_t *)(pos+i));}
	memcpy(&bin, data, sizeof(T));
	return bin;
}